#include "lidarIrisDescriptor.h"

lidar_iris_descriptor::lidar_iris_descriptor(
	int rows 					= 80,     //行数
	int columns 				= 360,    //列数
	int n_scan 					= 64,     //激光线束
	double distance_threshold 	= 0.32,   //距离阈值
	int exclude_recent_frame_num= 30,     //最近帧的排除数量
	int match_mode				= 2,      //匹配方向模式
	int candidates_num 			= 10,     //候选数量
	int nscale					= 4,      //表示一个比例值
	int min_wave_length 		= 18,     //最小波长
	float mult 					= 1.6,    //乘法因子
	float sigma_on_f 			= 0.75,   //表示sigma值
	int robot_num 				= 1,      //机器人数量
	int id 						= 0) :    //机器人id
	rows_(rows), // 80 in the original paper
	columns_(columns), // 360 in the original paper
	n_scan_(n_scan), // lidar sensor lines
	distance_threshold_(distance_threshold), // 0.10-0.45 is ok.
		// 0.10-0.15 is very fine (rare false-alarms);
		// 0.15-0.35 is good but ICP fitness score check should be required for robustness;
		// 0.35-0.45 is choice for using with robust outlier rejection.
	exclude_recent_frame_num_(exclude_recent_frame_num), // simply just keyframe gap
	match_mode_(match_mode), // 0 for detecting same direction
								// 1 for detecting opposite direction
								// 2 for detecting both same and opposite direction
	candidates_num_(candidates_num), // 6-10 is enough
	nscale_(nscale), // 4 in the original paper
	min_wave_length_(min_wave_length), // 18 in the original paper
	mult_(mult), // 1.6 in the original paper
	sigma_on_f_(sigma_on_f), // 0.75 in the original paper
	robot_num_(robot_num), // number of robot in robotic swarm
	id_(id) // this robot id
{
	// allocate memory
    // 分配内存
	for(int i = 0; i < robot_num_; i++)
	{
		unordered_map<int, lidar_iris_descriptor::featureDesc> base_feature_desc;//用于存储特征描述符
		unordered_map<int, Eigen::MatrixXf> base_matrix;    //用于存储矩阵
		unordered_map<int, int> base_int;   //用于存储整数值
		iris_features.push_back(base_feature_desc);     //将base_feature_desc添加到iris_features向量中
		iris_rowkeys.push_back(base_matrix);            //将base_matrix添加到iris_rowkeys向量中
		indexes_maps.push_back(base_int);               //将base_int添加到indexes_maps向量中
	}
}

lidar_iris_descriptor::~lidar_iris_descriptor()
{
	
}

// 是lidar_iris_descriptor命名空间中的一个函数getIris的实现。
// 该函数接收一个pcl::PointCloud<pcl::PointXYZI>类型的参数cloud，
// 并返回一个std::pair<Eigen::VectorXf, cv::Mat1b>类型的结果。
std::pair<Eigen::VectorXf, cv::Mat1b> lidar_iris_descriptor::getIris(
	const pcl::PointCloud<pcl::PointXYZI> &cloud)
{
    //全零cv::Mat1b类型变量iris_image，用于存储激光雷达的iris图像
    cv::Mat1b iris_image = cv::Mat1b::zeros(rows_, columns_);
    //全零Eigen::MatrixXf类型变量iris_row_key_matrix，用于存储iris行键的矩阵
	Eigen::MatrixXf iris_row_key_matrix = Eigen::MatrixXf::Zero(rows_, columns_);

	// extract lidar iris image
    // 根据n_scan_的值,通过不同的条件分支来提取激光雷达的iris图像和行键
	if(n_scan_ = 6) // livox
	{
		for(auto p : cloud.points)
		{
			float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]); // xy-plane distance
			float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 25; // [0, 26.9] deg.
			float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180; //atan2: (-pi, pi] rad.
			int Q_dis = std::min(std::max((int)floor(dis), 0), (rows_-1));
			int Q_arc = std::min(std::max((int)floor(arc / 3.3), 0), 7);
			int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), (columns_-1));
			iris_image.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
			if(iris_row_key_matrix(Q_dis, Q_yaw) < p.data[2])
			{
				iris_row_key_matrix(Q_dis, Q_yaw) = p.data[2]; // update for taking maximum value at that bin
			}
		}
	}
	else if(n_scan_ == 16) // VLP16
	{
		for(auto p : cloud.points)
		{
			float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]);
			float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 15; // [0, 30] deg.
			float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180;
			int Q_dis = std::min(std::max((int)floor(dis), 0), (rows_-1));
			int Q_arc = std::min(std::max((int)floor(arc / 4.0f), 0), 7);
			// int Q_arc = std::min(std::max((int)ceil(p.data[2] + 5), 0), 7);
			int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), (columns_-1));
			iris_image.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
			if(iris_row_key_matrix(Q_dis, Q_yaw) < p.data[2])
			{
				iris_row_key_matrix(Q_dis, Q_yaw) = p.data[2];
			}
		}
	}
	else if(n_scan_ == 64) // HDL64
	{
		for(auto p : cloud.points)
		{
			float dis = sqrt(p.data[0] * p.data[0] + p.data[1] * p.data[1]);
			// float arc = (atan2(p.data[2], dis) * 180.0f / M_PI) + 25; // [0, 26.9] deg.
			float yaw = (atan2(p.data[1], p.data[0]) * 180.0f / M_PI) + 180;
			int Q_dis = std::min(std::max((int)floor(dis), 0), (rows_-1));
			// int Q_arc = std::min(std::max((int)floor(arc / 3.3), 0), 7);
			int Q_arc = std::min(std::max((int)ceil(p.data[2] + 5), 0), 7);
			int Q_yaw = std::min(std::max((int)floor(yaw + 0.5), 0), (columns_-1));
			iris_image.at<uint8_t>(Q_dis, Q_yaw) |= (1 << Q_arc);
			if(iris_row_key_matrix(Q_dis, Q_yaw) < p.data[2])
			{
				iris_row_key_matrix(Q_dis, Q_yaw) = p.data[2];
			}
		}
	}

	// extract rowkey
    // 从iris_row_key_matrix中提取行键（row key）
	Eigen::VectorXf rowkey = Eigen::VectorXf::Zero(rows_);
	for(int i = 0; i < iris_row_key_matrix.rows(); i++)
	{
        // 循环遍历iris_row_key_matrix的每一行。对于每一行，将其赋值给Eigen::VectorXf类型的变量curr_row
		Eigen::VectorXf curr_row = iris_row_key_matrix.row(i);
        // rowkey中的每个元素都表示对应行的行键的平均值
		rowkey(i) = curr_row.mean();
	}

	return std::make_pair(rowkey, iris_image); //iris_image (激光雷达的iris图像)
    //返回一个包含 rowkey 和 iris_image 的 std::pair 对象
}

// 函数circRowShift的实现
// 接收一个cv::Mat类型的参数src和一个整数参数shift_m_rows，并返回一个cv::Mat类型的结果
inline cv::Mat lidar_iris_descriptor::circRowShift(
	const cv::Mat &src,
	int shift_m_rows)
{
    // 检查shift_m_rows是否等于0。如果是，表示不需要进行行位移，直接使用src.clone()创建并返回src的副本
	if(shift_m_rows == 0)
	{
		return src.clone();
	}

	shift_m_rows %= src.rows;
	int m = shift_m_rows > 0 ? shift_m_rows : src.rows + shift_m_rows;
    // 创建一个与src具有相同大小和类型的cv::Mat变量dst，用于存储位移后的图像
	cv::Mat dst(src.size(), src.type());

    //cv::Range是OpenCV库中的一个类，用于表示一个数值范围。它通常与cv::Mat结合使用，用于指定图像或矩阵的子区域
    //cv::Mat::copyTo是cv::Mat类的一个成员函数，用于将一个cv::Mat对象的一部分数据复制到另一个cv::Mat对象中。
    //使用cv::Range和cv::Mat::copyTo函数将src的部分行复制到dst中，实现行位移的效果
	src(cv::Range(src.rows - m, src.rows), cv::Range::all()).copyTo(dst(cv::Range(0, m), cv::Range::all()));
	src(cv::Range(0, src.rows - m), cv::Range::all()).copyTo(dst(cv::Range(m, src.rows), cv::Range::all()));
	// 返回经过行位移后的图像dst
    return dst;
}

// 实现了一个循环列位移的函数circColShift，
// 它接收一个cv::Mat类型的图像src和一个整数参数shift_n_cols
inline cv::Mat lidar_iris_descriptor::circColShift(
	const cv::Mat &src,
	int shift_n_cols)
{
	if(shift_n_cols == 0)
	{
		return src.clone();
	}
	shift_n_cols %= src.cols;
	int n = shift_n_cols > 0 ? shift_n_cols : src.cols + shift_n_cols;
	cv::Mat dst(src.size(), src.type());
	src(cv::Range::all(), cv::Range(src.cols - n, src.cols)).copyTo(dst(cv::Range::all(), cv::Range(0, n)));
	src(cv::Range::all(), cv::Range(0, src.cols - n)).copyTo(dst(cv::Range::all(), cv::Range(n, src.cols)));
    // 返回一个经过列位移后的图像cv::Mat对象
    return dst;
}

// 实现了一个循环位移的函数circShift，
// 它接收一个cv::Mat类型的图像src，以及两个整数参数shift_m_rows和shift_n_cols，
// 分别表示行和列的位移值。
// 函数通过调用circRowShift和circColShift函数实现了循环位移操作，并返回经过位移后的图像
cv::Mat lidar_iris_descriptor::circShift(
	const cv::Mat &src,
	int shift_m_rows,
	int shift_n_cols)
{
	return circColShift(circRowShift(src, shift_m_rows), shift_n_cols);
}

// 实现了一个对输入图像进行Log-Gabor滤波的函数logGaborFilter，
// 它接收一个单通道的浮点型图像src，
// 以及一些参数，包括尺度数nscale、最小波长min_wave_length、倍数mult和sigma_on_f。
std::vector<cv::Mat2f> lidar_iris_descriptor::logGaborFilter(
	const cv::Mat1f &src,
	unsigned int nscale,
	int min_wave_length,
	double mult,
	double sigma_on_f)
{
    // 输入图像的行数和列数
	int rows = src.rows;
	int cols = src.cols;
	cv::Mat2f filtersum = cv::Mat2f::zeros(1, cols);   //保存滤波器的和
	std::vector<cv::Mat2f> EO(nscale);
    // 如果ndata是奇数列，表示图像的列数为奇数，则执行以下操作将ndata减去1，使其成为偶数列
    // 在后续的计算中，涉及到DFT（离散傅里叶变换）和IDFT（逆离散傅里叶变换），通常要求图像的列数为偶数
	int ndata = cols;
	if(ndata % 2 == 1)
	{
		ndata--;
	}
	cv::Mat1f logGabor = cv::Mat1f::zeros(1, ndata);  //存储Log-Gabor滤波器
	cv::Mat2f result = cv::Mat2f::zeros(rows, ndata);   //存储滤波结果
	cv::Mat1f radius = cv::Mat1f::zeros(1, ndata / 2 + 1);  //表示直流分量的半径
    //实现了计算频域半径radius的操作，用于后续的Log-Gabor滤波器计算
	radius.at<float>(0, 0) = 1;
	for(int i = 1; i < ndata / 2 + 1; i++)
	{
		radius.at<float>(0, i) = i / (float)ndata;
	}

    // 初始化变量wavelength为min_wave_length，它表示当前尺度的波长
	double wavelength = min_wave_length;
    // 每次循环表示一个尺度的滤波器计算
	for(int s = 0; s < nscale; s++)
	{
        // 计算当前尺度的频率fo，并计算频率比rfo
		double fo = 1.0 / wavelength;
		double rfo = fo / 0.5;
		
		cv::Mat1f temp;     //用于存储计算过程中的临时结果 (radius.size());
        // 将频域半径radius除以fo得到radius_tmp，然后对其取对数（cv::log）并平方（cv::pow）得到temp
		cv::Mat1f radius_tmp = radius / fo;
		cv::log(radius_tmp, temp);
		cv::pow(temp, 2, temp);
        // 计算中间结果mid_result，
        // 再将其求指数（cv::exp）得到最终的Log-Gabor滤波器响应值
        // 并将其复制到logGabor矩阵的一部分
		cv::Mat1f mid_result = (-temp) / (2 * log(sigma_on_f) * log(sigma_on_f));
		cv::exp(mid_result, temp);
		temp.copyTo(logGabor.colRange(0, ndata / 2 + 1));

        // 将logGabor矩阵的第一个元素置为0，表示直流分量的滤波器响应为0   zhiliufenliang?????
		logGabor.at<float>(0, 0) = 0;
        // 创建复数滤波器filter，合并实部logGabor和虚部（全零）
		cv::Mat2f filter;
		cv::Mat1f filterArr[2] = {logGabor, cv::Mat1f::zeros(logGabor.size())};
		cv::merge(filterArr, 2, filter);
        // 将当前滤波器的响应值累加到filtersum矩阵中
		filtersum = filtersum + filter;

        //遍历输入图像的每一行，将每行的图像进行DFT变换（cv::dft），与滤波器filter相乘（cv::mulSpectrums），
        // 再进行IDFT逆变换（cv::idft），并将结果存储在result矩阵的相应行中
		for(int r = 0; r < rows; r++)
		{
			cv::Mat2f src2f;
			cv::Mat1f srcArr[2] = {src.row(r).clone(), cv::Mat1f::zeros(1, src.cols)};
			cv::merge(srcArr, 2, src2f);
			cv::dft(src2f, src2f);
			cv::mulSpectrums(src2f, filter, src2f, 0);
			cv::idft(src2f, src2f);
			src2f.copyTo(result.row(r));
		}
		EO[s] = result.clone(); //当前尺度的滤波结果存储在EO数组中
		wavelength *= mult;     //更新波长wavelength为当前波长乘以倍数mult
	}
    // 对filtersum进行列位移操作，使用circShift函数将其循环移动到中心位置
	filtersum = circShift(filtersum, 0, cols / 2);
	return EO;
}

// 一个lidar_iris_descriptor类的成员函数logFeatureEncode，用于对输入的灰度图像进行特征编码
//——————————
void lidar_iris_descriptor::logFeatureEncode(
	const cv::Mat1b &src,  //输入的灰度图像，数据类型为cv::Mat1b，即单通道的8位无符号整数矩阵//
	unsigned int nscale,   //尺度数目
	int min_wave_length,   //最小波长
	double mult,           //波长的倍数因子
	double sigma_on_f,     //高斯函数的标准差除以频率
	cv::Mat1b &T,          //输出矩阵，用于存储特征编码结果。
	cv::Mat1b &M)          //输出矩阵，用于存储特征编码结果
{
	cv::Mat1f src_float;  //将输入图像src转换为单精度浮点型矩阵src_float
	src.convertTo(src_float, CV_32FC1);
    // 调用logGaborFilter函数对src_float进行滤波，返回一个滤波结果的列表list
	auto list = logGaborFilter(src_float, nscale, min_wave_length, mult, sigma_on_f);
    // 代码创建了两个向量列表Tlist和Mlist，它们的大小分别为nscale * 2。
    // 这些列表将用于存储特征编码的中间结果
	std::vector<cv::Mat1b> Tlist(nscale * 2), Mlist(nscale * 2);
	for (int i = 0; i < list.size(); i++)
	{
        // 数组arr，用于存储分离后的实部和虚部。
		cv::Mat1f arr[2];
        // 使用cv::split函数将滤波结果list[i]分离为实部和虚部，并存储到数组arr中
		cv::split(list[i], arr);
        // 将实部大于0的部分存储到Tlist中的相应位置（Tlist[i]）
        // 将实部小于0的部分存储到Tlist中的相应位置（Tlist[i + nscale]，偏移nscale个位置）。
        // ????
		Tlist[i] = arr[0] > 0;
		Tlist[i + nscale] = arr[1] > 0;

        // 实部和虚部的幅度，并存储到新创建的单通道浮点型矩阵m中
		cv::Mat1f m;
		cv::magnitude(arr[0], arr[1], m);
        // 将幅度小于阈值0.0001的部分存储到Mlist中的相应位置（Mlist[i]）。
        // 将幅度小于阈值0.0001的部分存储到Mlist中的相应位置（Mlist[i + nscale]，偏移nscale个位置）。
                //   ？？？？
		Mlist[i] = m < 0.0001;
		Mlist[i + nscale] = m < 0.0001;
	}
    // 将Tlist中的所有矩阵按垂直方向拼接，结果存储到输出矩阵T中
    // 将Mlist中的所有矩阵按垂直方向拼接，结果存储到输出矩阵M中
	cv::vconcat(Tlist, T);
	cv::vconcat(Mlist, M);
}

// 是lidar_iris_descriptor类中的成员函数getFeature的实现。
// 该函数接受一个输入图像src，并返回一个名为featureDesc的结构体desc
// 其中包含了输入图像src以及通过调用logFeatureEncode函数生成的特征编码结果矩阵T和M
lidar_iris_descriptor::featureDesc lidar_iris_descriptor::getFeature(
	const cv::Mat1b &src)
{
	lidar_iris_descriptor::featureDesc desc;  //创建desc用于存储特征描述信息
	desc.img = src;          //将输入图像保存到desc中
    // 调用logFeatureEncode函数，
    // 传递参数src以及其他类成员变量nscale_、min_wave_length_、mult_和sigma_on_f_，对输入图像进行特征编码。
    // 将logFeatureEncode函数生成的特征编码结果矩阵T和M分别赋值给desc中的成员变量T和M
	logFeatureEncode(src, nscale_, min_wave_length_, mult_, sigma_on_f_, desc.T, desc.M);
    // 返回包含特征描述信息的desc结构体
	return desc;
}

// 是lidar_iris_descriptor类的成员函数getFeature的重载版本，用于从输入的灰度图像中提取特征描述信息
// 该函数接受一个输入图像src和一个浮点型向量vec的引用，
// 这个getFeature函数在提取特征描述信息之前，对输入图像进行了一些预处理操作，包括图像类型转换和行方向的平均化处理
lidar_iris_descriptor::featureDesc lidar_iris_descriptor::getFeature(
	const cv::Mat1b &src,
	std::vector<float> &vec)
{
    // 创建一个临时的单精度浮点型矩阵temp，并将输入图像src转换为单精度浮点型
	cv::Mat1f temp;
    // 调用convertTo函数，将src转换为单精度浮点型（CV_32FC1）矩阵temp后
	src.convertTo(temp, CV_32FC1);
    // 使用cv::reduce函数对二值化矩阵进行行方向的均值操作，
    // 将每一行的非零元素平均值计算为一行，并存储在temp矩阵中
	cv::reduce((temp != 0) / 255, temp, 1, cv::REDUCE_AVG);
    // 检查temp矩阵是否是连续存储的（即是否可以被视为一维向量），
    // 如果是，则直接将temp赋值给向量vec；
    // 否则，使用clone函数创建temp矩阵的副本，赋值给向量vec
	vec = temp.isContinuous() ? temp : temp.clone();
    // 调用另一个重载的getFeature函数，传递输入图像和向量vec作为参数，获取特征描述信息并返回
	return getFeature(src);
}

// 是lidar_iris_descriptor类中的成员函数recomb的实现。
// 该函数接受两个参数，src和dst，分别表示输入图像和输出图像
void lidar_iris_descriptor::recomb(
	cv::Mat &src,
	cv::Mat &dst) // Recombinate image quaters
{
	int cx = src.cols >> 1;
	int cy = src.rows >> 1;
	cv::Mat tmp;
	tmp.create(src.size(), src.type());
	src(cv::Rect(0, 0, cx, cy)).copyTo(tmp(cv::Rect(cx, cy, cx, cy)));
	src(cv::Rect(cx, cy, cx, cy)).copyTo(tmp(cv::Rect(0, 0, cx, cy)));
	src(cv::Rect(cx, 0, cx, cy)).copyTo(tmp(cv::Rect(0, cy, cx, cy)));
	src(cv::Rect(0, cy, cx, cy)).copyTo(tmp(cv::Rect(cx, 0, cx, cy)));
    // recomb函数实现了对输入图像四个象限的重新排列，
    // 将左上角象限移动到右下角，右下角移动到左上角，右上角移动到左下角，左下角移动到右上角
	dst = tmp;
}

// 是lidar_iris_descriptor类中的成员函数forwardFFT的实现
// forwardFFT函数实现了对输入图像的2D正向快速傅里叶变换，并将变换结果存储在f_img中。
void lidar_iris_descriptor::forwardFFT(
	cv::Mat &src,
	cv::Mat *f_img,
	bool do_recomb = true) // 2D Forward FFT
{
    // 根据输入图像的行数和列数，获取最佳的DFT尺寸，并将其分别赋值给变量M和N
	int M = cv::getOptimalDFTSize(src.rows);
	int N = cv::getOptimalDFTSize(src.cols);
	cv::Mat padded;
    // 使用cv::copyMakeBorder函数将输入图像src进行边界填充，将填充后的图像存储到padded中
    // 填充的方式为cv::BORDER_CONSTANT，边界值为0，
    // 填充的上、下、左、右的像素数分别为0、M - src.rows、0、N - src.cols
	copyMakeBorder(src, padded, 0, M - src.rows, 0, N - src.cols, cv::BORDER_CONSTANT, cv::Scalar::all(0));
	cv::Mat planes[] = { cv::Mat_<float>(padded), cv::Mat::zeros(padded.size(), CV_32F) };
    //两个cv::Mat对象：planes[0]和planes[1]
	cv::Mat complex_img;// 用于存储合并后的复数图像
	merge(planes, 2, complex_img);  // 将planes中的实部和虚部合并为一个复数图像complex_img
    // 使用cv::dft函数对complex_img进行离散傅里叶变换（DFT），将变换结果保存在complex_img中
	dft(complex_img, complex_img);
    // 将complex_img分离为实部和虚部，分别存储在planes[0]和planes[1]中
	split(complex_img, planes);
    // planes[0]和planes[1]，用于存储实部和虚部
    // 第一行和第二行使用位与运算符 & 和负数值 -2 将 planes[0] 和 planes[1] 的尺寸调整为偶数值。
    // 这是通过对 planes[0] 和 planes[1] 的行数和列数进行位与运算，将其最后一位的值置为零来实现的。
    // 为了确保尺寸是偶数，以满足傅里叶变换的要求
	planes[0] = planes[0](cv::Rect(0, 0, planes[0].cols & -2, planes[0].rows & -2));
	planes[1] = planes[1](cv::Rect(0, 0, planes[1].cols & -2, planes[1].rows & -2));
	// 如果 do_recomb 为 true，则执行以下步骤：
    // 调用 recomb 函数对 planes[0] 进行重新组合操作
    // 调用 recomb 函数对 planes[1] 进行重新组合操作
    if(do_recomb)
	{
		recomb(planes[0], planes[0]);
		recomb(planes[1], planes[1]);   //recomb函数实现了对输入图像四个象限的重新排列
	}
	planes[0] /= float(M*N); //除以 float(M*N)，以归一化变换结果
	planes[1] /= float(M*N);
    // 将 planes[0] 克隆到 f_img[0] 中，将 planes[1] 克隆到 f_img[1] 中
	f_img[0] = planes[0].clone();
	f_img[1] = planes[1].clone();
}

// 实现了一个高通滤波器，用于生成一个高通滤波器的核矩阵。该核矩阵被存储在 dst 中
void lidar_iris_descriptor::highpass(cv::Size sz, cv::Mat& dst)
{
	cv::Mat a = cv::Mat(sz.height, 1, CV_32FC1);
    // 创建一个 cv::Mat 对象 a，其尺寸为 sz.height 行 1 列，数据类型为 CV_32FC1（单通道浮点型）
	cv::Mat b = cv::Mat(1, sz.width, CV_32FC1);
    //创建一个 cv::Mat 对象 b，其尺寸为 1 行 sz.width 列，数据类型为 CV_32FC1

    // 计算步长 step_y，即 CV_PI / sz.height，用于在后续循环中生成 a 的值
	float step_y = CV_PI / sz.height;
	float val = -CV_PI*0.5;  // 初始化变量 val 为 -CV_PI*0.5

    // 使用循环从 0 到 sz.height-1，生成 a 的值。
    // 在每次迭代中，计算 cos(val) 并将其存储在 a 的相应位置，然后将 val 增加 step_y。
	for(int i = 0; i < sz.height; ++i)
	{
		a.at<float>(i) = cos(val);
        // 计算 cos(val) 并将其存储在 a 的相应位置，然后将 val 增加 step_y。
		val += step_y;
	}

    // 将 val 重新初始化为 -CV_PI*0.5。
    // 计算步长 step_x，即 CV_PI / sz.width，用于在后续循环中生成 b 的值
	val = -CV_PI*0.5;
	float step_x = CV_PI / sz.width;
	for(int i = 0; i < sz.width; ++i)
	{
		b.at<float>(i) = cos(val);
        // 使用循环从 0 到 sz.width-1，生成 b 的值。在每次迭代中，计算 cos(val) 并将其存储在 b 的相应位置，然后将 val 增加 step_x
		val += step_x;
	}

    // 一个临时的 cv::Mat 对象 tmp，其值为矩阵 a 与矩阵 b 的乘积
	cv::Mat tmp = a*b;
    // 计算 1.0 - tmp，然后将结果与 2.0 - tmp 进行逐元素相乘。    // ？？？？
	dst = (1.0 - tmp).mul(2.0 - tmp);
}

// 该函数实现了对输入图像进行对数极坐标变换，并将变换后的图像存储在输出图像 dst 中
// 同时，还返回了对数极坐标变换的基数 log_base，该值可以用于逆变换或其他相关操作
float lidar_iris_descriptor::logpolar(
	cv::Mat& src,
	cv::Mat& dst)
{
	float radii = src.cols;  //输入图像的宽度和高度
	float angles = src.rows;
	cv::Point2f center(src.cols / 2, src.rows / 2);  //图像中心点的坐标，存储在 center 中
	float d = cv::norm(cv::Vec2f(src.cols - center.x, src.rows - center.y));
    // 使用 cv::norm 函数计算从图像中心到图像右下角的欧氏距离
	float log_base = std::pow(10.0, log10(d) / radii);
    // 计算对数极坐标变换的基数 log_base
    // 使用 std::pow 函数计算以 10 为底的 d 的对数除以 radii 的结果的幂次
    // std::pow 是 C++ 标准库中的一个函数，用于计算一个数的指定次幂。
	float d_theta = CV_PI / (float)angles;  //计算了每个角度步长 d_theta
	float theta = CV_PI / 2.0;              //定义了初始角度 theta，它被设置为 CV_PI（圆周率）除以 2.0
	float radius = 0;                       //定义了初始半径 radius，它被设置为0
    // 创建了两个 cv::Mat 对象 map_x 和 map_y，
    // 它们的大小与 src 的大小相同，数据类型为 CV_32FC1
	cv::Mat map_x(src.size(), CV_32FC1);
	cv::Mat map_y(src.size(), CV_32FC1);
    // 循环遍历每个角度和半径
    // 这段代码通过计算极坐标每个点的坐标，并将其存储在 map_x 和 map_y 两个矩阵中
    // 即极坐标变换为笛卡尔坐标
	for(int i = 0; i < angles; ++i)
	{
		for(int j = 0; j < radii; ++j)
		{
			radius = std::pow(log_base, float(j));
			float x = radius * sin(theta) + center.x;
			float y = radius * cos(theta) + center.y;
			map_x.at<float>(i, j) = x;
			map_y.at<float>(i, j) = y;
		}
		theta += d_theta;
	}
    // 使用 cv::remap 函数将源图像 src 根据计算得到的映射关系 map_x 和 map_y 进行重映射，
    // 并将结果存储在目标图像 dst 中。
    // cv::INTER_LINEAR 是插值方法，用于在映射过程中对像素进行插值。
    // cv::BORDER_CONSTANT 是边界模式，用于指定在映射过程中超出图像边界的像素的处理方式。
    // cv::Scalar(0, 0, 0) 是用于填充超出边界的像素的颜色。
	cv::remap(src, dst, map_x, map_y, cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar(0, 0, 0));
	return log_base;
}

// 代码是lidar_iris_descriptor类中logPolarFFTTemplateMatch函数的实现，该函数使用对数极坐标傅里叶变换进行模板匹配
cv::RotatedRect lidar_iris_descriptor::logPolarFFTTemplateMatch(
	cv::Mat& im0,
	cv::Mat& im1/*, double canny_threshold1, double canny_threshold2*/)
{
	// Accept 1 or 3 channel CV_8U, CV_32F or CV_64F images.
    // 使用 CV_Assert 函数来检查输入图像 im0 的类型，
    // 确保它是 1 通道或 3 通道的 CV_8U、CV_32F 或 CV_64F 类型之一
	CV_Assert((im0.type() == CV_8UC1) || (im0.type() == CV_8UC3) ||
		(im0.type() == CV_32FC1) || (im0.type() == CV_32FC3) ||
		(im0.type() == CV_64FC1) || (im0.type() == CV_64FC3));

    // 使用 CV_Assert 函数来检查输入图像 im0 和 im1 的尺寸是否相同
	CV_Assert(im0.rows == im1.rows && im0.cols == im1.cols);

    // 使用 CV_Assert 断言函数来检查输入图像 im0 和 im1 的通道数是否为 1、3 或 4
    // ？？？？？
	CV_Assert(im0.channels() == 1 || im0.channels() == 3 || im0.channels() == 4);

	CV_Assert(im1.channels() == 1 || im1.channels() == 3 || im1.channels() == 4);

	//cv::Mat im0_tmp = im0.clone(); //  ？？？？？
	//cv::Mat im1_tmp = im1.clone();

    //这四个代码块用于将输入图像 im0 和 im1 转换为灰度图像，如果它们的通道数为 3 或 4
	if(im0.channels() == 3)
	{
		cv::cvtColor(im0, im0, cv::ColorConversionCodes::COLOR_BGR2GRAY);
	}
	if(im0.channels() == 4)
	{
		cv::cvtColor(im0, im0, cv::ColorConversionCodes::COLOR_BGRA2GRAY);
	}
	if(im1.channels() == 3)
	{
		cv::cvtColor(im1, im1, cv::ColorConversionCodes::COLOR_BGR2GRAY);
	}
	if(im1.channels() == 4)
	{
		cv::cvtColor(im1, im1, cv::ColorConversionCodes::COLOR_BGRA2GRAY);
	}

    // 这四个代码块用于将输入图像 im0 和 im1 的数据类型转换为 CV_8UC1 类型，
    // 如果它们的数据类型为 CV_32FC1 或 CV_64FC1。转换的过程中，
    // 图像的像素值会乘以一个缩放因子，使得像素值范围在 0 到 255 之间。
	if(im0.type() == CV_32FC1)
	{
		im0.convertTo(im0, CV_8UC1, 255.0);
	}
	if(im1.type() == CV_32FC1)
	{
		im1.convertTo(im1, CV_8UC1, 255.0);
	}
	if(im0.type() == CV_64FC1)
	{
		im0.convertTo(im0, CV_8UC1, 255.0);
	}
	if(im1.type() == CV_64FC1)
	{
		im1.convertTo(im1, CV_8UC1, 255.0);
	}

	// Canny(im0, im0, canny_threshold1, canny_threshold2); // you can change this
	// Canny(im1, im1, canny_threshold1, canny_threshold2);
	
	// Ensure both images are of CV_32FC1 type
    // 将输入图像 im0 和 im1 的数据类型转换为 CV_32FC1 类型，并进行归一化，使像素值范围在 0 到 1 之间
	im0.convertTo(im0, CV_32FC1, 1.0 / 255.0);
	im1.convertTo(im1, CV_32FC1, 1.0 / 255.0);

    //使用了 forwardFFT 函数对输入图像 im0 和 im1 进行傅里叶变换，并计算了频谱 f0 和 f1
	cv::Mat F0[2], F1[2];
	cv::Mat f0, f1;
	forwardFFT(im0, F0);
	forwardFFT(im1, F1);
	cv::magnitude(F0[0], F0[1], f0);
	cv::magnitude(F1[0], F1[1], f1);

	// Create filter
    // 创建了一个滤波器 h，用于高通滤波
	cv::Mat h;
	highpass(f0.size(), h);

	// Apply it in freq domain
    // 将频谱 f0 和 f1 分别与滤波器 h 相乘，进行频域滤波
	f0 = f0.mul(h);
	f1 = f1.mul(h);

    //使用 logpolar 函数对滤波后的频谱 f0 和 f1 进行对数极坐标变换
	float log_base;
	cv::Mat f0lp, f1lp;


	log_base = logpolar(f0, f0lp);
	log_base = logpolar(f1, f1lp);

	// Find rotation and scale
    // 使用 phaseCorrelate 函数计算图像 f1lp 和 f0lp 之间的相位相关性，得到旋转和缩放的信息

	cv::Point2d rotation_and_scale = cv::phaseCorrelate(f1lp, f0lp);
    // 根据相位相关性的结果计算旋转角度 angle 和缩放比例 scale
	float angle = 180.0 * rotation_and_scale.y / f0lp.rows;
	float scale = pow(log_base, rotation_and_scale.x);

    // 如果计算得到的缩放比例 scale 大于 1.8，则重新计算相位相关性，并将缩放比例取倒数。
    // 如果重新计算后的缩放比例仍大于 1.8，则输出错误信息并返回一个空的 cv::RotatedRect 对象
	if(scale > 1.8)
	{
		rotation_and_scale = cv::phaseCorrelate(f1lp, f0lp);
		angle = -180.0 * rotation_and_scale.y / f0lp.rows;
		scale = 1.0 / pow(log_base, rotation_and_scale.x);
		if (scale > 1.8)
		{
			std::cout << "Images are not compatible. Scale change > 1.8" << std::endl;
			return cv::RotatedRect();
		}
	}

	// 这段代码用于调整旋转角度 angle 的值，使其在 -90 到 90 度之间
	if(angle < -90.0)
	{
		angle += 180.0;
	}
	else if(angle > 90.0)
	{
		angle -= 180.0;
	}

	// Now rotate and scale fragment back, then find translation
    // 使用 getRotationMatrix2D 函数根据旋转角度 angle 和缩放比例 scale 创建一个旋转矩阵 rot_mat
	cv::Mat rot_mat = cv::getRotationMatrix2D(cv::Point(im1.cols / 2, im1.rows / 2), angle, 1.0 / scale);
    // angle表示旋转角度  ； 1.0 / scale 缩放因子

	// rotate and scale
    // 使用 warpAffine 函数对图像 im1 进行旋转和缩放，得到旋转缩放后的图像 im1_rs
	cv::Mat im1_rs;
	cv::warpAffine(im1, im1_rs, rot_mat, im1.size());

	// find translation
    // 使用 phaseCorrelate 函数计算旋转缩放后的图像 im1_rs 和原始图像 im0 之间的相位相关性，得到平移的信息
	cv::Point2d tr = cv::phaseCorrelate(im1_rs, im0);

	// compute rotated rectangle parameters
    // 用于计算旋转矩形的参数，包括中心点坐标、角度和大小
	cv::RotatedRect rr;
	rr.center = tr + cv::Point2d(im0.cols / 2, im0.rows / 2);
	rr.angle = -angle;
	rr.size.width = im1.cols / scale;
	rr.size.height = im1.rows / scale;

	//im0 = im0_tmp.clone();   // ？？？？
	//im1 = im1_tmp.clone();

    //函数返回计算得到的旋转矩形对象 rr
	return rr;
}

 // 调用 logPolarFFTTemplateMatch 函数，并将深拷贝后的输入图像作为参数传递给该函数，
// 然后将 logPolarFFTTemplateMatch 的返回结果返回给调用者
cv::RotatedRect lidar_iris_descriptor::fftMatch(
	const cv::Mat& im0,
	const cv::Mat& im1)
{
	cv::Mat im0_tmp = im0.clone();
	cv::Mat im1_tmp = im1.clone();
	return logPolarFFTTemplateMatch(im0_tmp, im1_tmp);
}

// 计算两个二进制图像之间的汉明距离（Hamming Distance）的函数
// 函数接受四个参数：T1、M1、T2、M2，分别表示两个二进制图像的模板图像、模板图像的掩码、目标图像和目标图像的掩码
// scale 是一个整数参数，表示位移的尺度。
// dis 是一个浮点数引用，用于存储计算得到的汉明距离。
// bias 是一个整数引用，用于存储使汉明距离最小的位移偏移。
void lidar_iris_descriptor::getHammingDistance(
	const cv::Mat1b &T1,
	const cv::Mat1b &M1,
	const cv::Mat1b &T2,
	const cv::Mat1b &M2,
	int scale,
	float &dis,
	int &bias)
{
	dis = NAN;       // 初始化 dis 为 NAN，表示距离无效
	bias = -1;       // bias 初始化为 -1
	// #pragma omp parallel for num_threads(8)
    // 循环结束后，dis 将包含计算得到的最小汉明距离，而 bias 将包含使最小汉明距离的位移偏移
	for(int shift = scale - 2; shift <= scale + 2; shift++)
	{
        //  通过调用 circShift 函数对模板图像 T1 和掩码图像 M1 进行位移，得到位移后的图像 T1s 和 M1s
		cv::Mat1b T1s = circShift(T1, 0, shift);
		cv::Mat1b M1s = circShift(M1, 0, shift);
        // 通过按位或运算符 | 结合 M1s 和目标图像的掩码 M2，生成一个新的掩码图像 mask
		cv::Mat1b mask = M1s | M2;
        // 将 mask 的像素值除以 255，得到一个二值掩码 mask_tmp，
        // 用于计算位移后图像中有效像素的数量，即非掩码区域的像素数目 MaskBitsNum
		cv::Mat1b mask_tmp = mask / 255;
		int MaskBitsNum = cv::sum(mask_tmp)[0];
        // 计算位移后图像中的总比特数 totalBits，即位移后的图像像素总数减去掩码像素的数量
		int totalBits = T1s.rows * T1s.cols - MaskBitsNum;
        // 对位移后的模板图像 T1s 和目标图像 T2 进行按位异或运算 ^，生成一个比特差异图像 C
		cv::Mat1b C = T1s ^ T2;
        // 将 C 与掩码的补集进行按位与运算 & ~mask，得到有效的比特差异图像
		C = C & ~mask;
        // 将有效的比特差异图像除以 255，得到一个二值图像 c_tmp，
        // 用于计算有效比特差异的像素数目 bitsDiff
		cv::Mat1b c_tmp = C / 255;
		int bitsDiff = cv::sum(c_tmp)[0];
        // 如果总比特数 totalBits 为 0，即位移后的图像没有有效的像素，那么将 dis 设置为 NAN，表示距离无效
		if(totalBits == 0)
		{
			dis = NAN;
		}
        // 否则，计算当前的汉明距离 currentDis，即有效比特差异的像素数目除以总比特数
		else
		{
			float currentDis = bitsDiff / (float)totalBits;
             // 如果当前的汉明距离小于 dis 或者 dis 是 NAN，则更新 dis 和 bias
			if(currentDis < dis || isnan(dis))
			{
				dis = currentDis;
				bias = shift;
			}
		}
	}
	return;
}

// 是一个比较函数，用于计算两个激光雷达虹膜描述符之间的相似性。
// 采用两个激光雷达虹膜描述符 img1 和 img2 作为输入，并且返回一个浮点数表示它们之间的相似度。
// 同时，如果传入了 bias 指针，则会将位移偏移值存储在 bias 变量中
float lidar_iris_descriptor::compare(
	const lidar_iris_descriptor::featureDesc &img1,
	const lidar_iris_descriptor::featureDesc &img2,
	int *bias)
{
    // 在 match_mode_ 等于2时执行的部分。它包含了一个循环，在每次循环中进行两次匹配，并比较它们的距离结果
	if(match_mode_==2)
	{
        // dis1 和 bias1 用于存储第一次匹配的距离和偏移值，
        // dis2 和 bias2 用于存储第二次匹配的距离和偏移值
		float dis1;
		int bias1;
		float dis2 = 0;
		int bias2 = 0;
		// #pragma omp parallel for num_threads(8)
		for(int i = 0; i < 2; i++)
		{
            // 如果 i 等于0，表示第一次循环，执行以下代码块
			if(i == 0)
			{
                // 首先调用了函数 fftMatch 对 img2.img 和 img1.img 进行匹配，并得到一个矩形区域 firstRect
				auto firstRect = fftMatch(img2.img, img1.img);
                // 计算位移偏移值 firstShift，通过将 firstRect.center.x 减去 img1.img.cols / 2 而得到
				int firstShift = firstRect.center.x - img1.img.cols / 2;
                // 调用函数 getHammingDistance，
                // 计算了 img1.T、img1.M、img2.T 和 img2.M 之间的汉明距离，并将结果存储在 dis1 和 bias1 中
				getHammingDistance(img1.T, img1.M, img2.T, img2.M, firstShift, dis1, bias1);
			}
            // 如果 i 不等于0，表示第二次循环，执行以下代码块
			else
			{
                // 首先调用了函数 circShift，对 img2.T、img2.M 和 img2.img 进行位移操作，
                // 得到了对应的 T2x、M2x 和 img2x
				auto T2x = circShift(img2.T, 0, 180);
				auto M2x = circShift(img2.M, 0, 180);
				auto img2x = circShift(img2.img, 0, 180);

				// 调用函数 fftMatch 对 img2x 和 img1.img 进行匹配，并得到一个矩形区域 secondRect
				auto secondRect = fftMatch(img2x, img1.img);
                // 计算了位移偏移值 secondShift，通过将 secondRect.center.x 减去 img1.img.cols / 2 而得到
				int secondShift = secondRect.center.x - img1.img.cols / 2;
                // 调用函数 getHammingDistance，
                // 计算了 img1.T、img1.M、T2x 和 M2x 之间的汉明距离，并将结果存储在 dis2 和 bias2 中
				getHammingDistance(img1.T, img1.M, T2x, M2x, secondShift, dis2, bias2);
			}
		}

        // 根据两次匹配的距离结果设置偏移值，并返回匹配的距离。
		if (dis1 < dis2)
		{
            // 如果 bias 非空，将 bias1 的值设置为指针所指向的变量的值。然后，返回 dis1 的值作为匹配的距离结果
			if (bias)
				*bias = bias1;
			return dis1;
		}
		else
		{
            // 如果 bias 非空，将 (bias2 + 180) % 360 的值设置为指针所指向的变量的值。
            // 这里使用了 (bias2 + 180) % 360 的计算是为了将偏移值转换到一个在0到359之间的范围内。
            // 然后，返回 dis2 的值作为匹配的距离结果
			if (bias)
				*bias = (bias2 + 180) % 360;
			return dis2;
		}
	}

    // 当 match_mode_ 为1时，根据不同的位移操作和匹配计算，得到匹配的距离 dis2 和偏移值 bias2，
    // 并将偏移值存储在 bias 指针所指向的变量中，最后返回 dis2 的值作为结果
	if(match_mode_==1)
	{
		auto T2x = circShift(img2.T, 0, 180);
		auto M2x = circShift(img2.M, 0, 180);
		auto img2x = circShift(img2.img, 0, 180);

		auto secondRect = fftMatch(img2x, img1.img);
		int secondShift = secondRect.center.x - img1.img.cols / 2;
		float dis2 = 0;
		int bias2 = 0;
		getHammingDistance(img1.T, img1.M, T2x, M2x, secondShift, dis2, bias2);
		if (bias)
			*bias = (bias2 + 180) % 360;
		return dis2;
	}

    // 当 match_mode_ 为0时，根据不同的匹配计算和位移偏移值，得到匹配的距离 dis1 和偏移值 bias1，
    // 将偏移值存储在 bias 指针所指向的变量中，最后返回 dis1 的值作为结果
	if(match_mode_==0)
	{
		auto firstRect = fftMatch(img2.img, img1.img);
		int firstShift = firstRect.center.x - img1.img.cols / 2;
		float dis1;
		int bias1;
		getHammingDistance(img1.T, img1.M, img2.T, img2.M, firstShift, dis1, bias1);
		if (bias)
			*bias = bias1;
		return dis1;
	}
}

// user-side API
// 是一个函数 saveDescriptorAndKey 的实现，它保存激光雷达的 iris 描述符和关键信息
// 函数接受以下参数：
// iris：一个指向浮点型数组的指针，表示 iris 描述符。
// robot：一个表示机器人标识符的整数。
// index：一个表示描述符索引的整数
void lidar_iris_descriptor::saveDescriptorAndKey(
	const float* iris,
	const int8_t robot,
	const int index)
{
    //创建一个大小为 rows_ 行 columns_ 列的单通道（灰度）图像 iris_map，并将所有像素值初始化为0
	cv::Mat1b iris_map = cv::Mat1b::zeros(rows_, columns_);
    // 创建一个大小为 rows_ 的 Eigen 向量 rowkey，并将所有元素初始化为0。
	Eigen::VectorXf rowkey = Eigen::VectorXf::Zero(rows_);

    // 使用两个嵌套的循环，将 iris 数组中的值按顺序赋给 iris_map 的像素值。
    // 循环变量 row_idx 和 col_idx 分别表示行和列的索引
	for(int row_idx = 0; row_idx < iris_map.rows; row_idx++)
	{
		for(int col_idx = 0; col_idx < iris_map.cols; col_idx++)
		{
			iris_map(row_idx, col_idx) = iris[row_idx*(iris_map.cols)+col_idx];
		}
	}

    // 将 iris 数组中的值赋给 rowkey。循环变量 row_idx 表示行的索引，通过相应的计算从 iris 数组中获取值。
	for(int row_idx = 0; row_idx < iris_map.rows; row_idx++)
	{
		rowkey(row_idx) = iris[row_idx+iris_map.rows*iris_map.cols];
	}

    // 调用 save 函数，将 iris_map、rowkey、robot 和 index 作为参数，保存描述符和关键信息
	save(iris_map, rowkey, robot, index);
}

// 函数 save 的实现，它用于保存激光雷达的 iris 描述符和关键信息。
// 函数接受以下参数：
// iris：一个 cv::Mat1b 类型的单通道图像，表示 iris 描述符。
// rowkey：一个 Eigen::MatrixXf 类型的矩阵，表示关键信息。
// robot：一个表示机器人标识符的整数。
// index：一个表示描述符索引的整数。
//   怎么实现//
void lidar_iris_descriptor::save(
	const cv::Mat1b iris,
	Eigen::MatrixXf rowkey,
	const int8_t robot,
	const int index)
{
	std::vector<float> rowKeyVec;
    // 调用 getFeature 函数，传入 iris 图像作为参数，获取 iris 描述符的特征
	auto irisFeature = getFeature(iris);

	// iris freature (descriptor) for single robot
    // 将 iris 描述符的特征 irisFeature 以键值对的形式添加到 iris_features 容器中，
    // 键是机器人标识符 robot 和描述符索引 index。这样可以将每个机器人的 iris 描述符特征存储起来。
	iris_features[robot].emplace(make_pair(index, irisFeature));

	// rowkey for knn search
    // 对 iris_rowkeys 容器的调整大小和赋值操作
	// iris_rowkeys[robot].conservativeResize(rows_, iris_features[robot].size());
	// iris_rowkeys[robot].block(0, iris_features[robot].size()-1, rows_, 1) = rowkey.block(0, 0, rows_, 1);
	// 将关键信息 rowkey 以键值对的形式添加到 iris_rowkeys 容器中，
    // 键是机器人标识符 robot 和描述符索引 index。这样可以将每个机器人的关键信息与对应的描述符关联起来。
    iris_rowkeys[robot].emplace(make_pair(index, rowkey));

	// trasform local index to global
    // 将描述符索引 index 与 iris 描述符全局索引的大小相对应的值以键值对的形式添加到 indexes_maps 容器中，
    // 键是机器人标识符 robot 和描述符索引 index。这样可以将局部索引转换为全局索引，方便后续的检索和索引操作。
	indexes_maps[robot].emplace(make_pair(index, iris_feature_index_pairs.size()));
    // iris_feature_index_pairs向量的大小

	// descriptor global index
    // 将机器人标识符 robot 和描述符索引 index 以键值对的形式添加到 iris_feature_index_pairs 容器中。
    // 这样可以获得描述符的全局索引，用于唯一标识每个描述符
	iris_feature_index_pairs.push_back(std::make_pair(robot,index)); //index
}

// 函数 makeAndSaveDescriptorAndKey 的实现，
// 它用于生成并保存激光雷达的 iris 描述符和关键信息，并返回描述符的数据
// 函数接受以下参数：
// scan：一个 pcl::PointCloud<pcl::PointXYZI> 类型的点云数据，表示激光雷达的扫描结果。
// robot：一个表示机器人标识符的整数。
// index：一个表示描述符索引的整数。
//  ？？？ 前面存在saveDescriptorAndKey函数
std::vector<float> lidar_iris_descriptor::makeAndSaveDescriptorAndKey(
	const pcl::PointCloud<pcl::PointXYZI>& scan,
	const int8_t robot,
	const int index)
{
    // 调用 getIris 函数，传入 scan 点云数据作为参数，获取 iris 描述符的图像和关键信息
	auto scan_iris_image = getIris(scan);
    // 调用 save 函数，传入 iris 描述符的图像和关键信息，以及机器人标识符和描述符索引，保存描述符和关键信息。
	save(scan_iris_image.second, scan_iris_image.first, robot, index);
	std::vector<float> descriptor_msg_data; //存储描述符的数据

    // 使用两个嵌套的循环，将 iris 描述符图像中的像素值按顺序添加到 descriptor_msg_data 向量中。
    // 循环变量 row_idx 和 col_idx 分别表示行和列的索引。
	for(int row_idx = 0; row_idx < scan_iris_image.second.rows; row_idx++)
	{
		for(int col_idx = 0; col_idx < scan_iris_image.second.cols; col_idx++)
		{
			descriptor_msg_data.push_back(scan_iris_image.second(row_idx, col_idx));
		}
	}

    // 使用循环，将关键信息中每行的值添加到 descriptor_msg_data 向量中。循环变量 row_idx 表示行的索引。
	for(int row_idx = 0; row_idx < scan_iris_image.second.rows; row_idx++)
	{
		descriptor_msg_data.push_back(scan_iris_image.first(row_idx));
	}

	// 返回存储描述符数据的 descriptor_msg_data 向量
	return descriptor_msg_data;
}

// 函数 detectIntraLoopClosureID 的实现，用于检测当前帧与历史帧之间的内部循环闭合
// 该函数主要利用 KD 树进行最近邻搜索，计算当前帧与历史帧之间的相似度，并根据阈值判断是否存在内部循环闭合。
std::pair<int, float> lidar_iris_descriptor::detectIntraLoopClosureID(
	const int cur_ptr)
{
    // 初始化一个 std::pair<int, float> 类型的变量 result，并将其值设置为 {-1, 0.0}，表示默认的检测结果
	std::pair<int, float> result {-1, 0.0};
    // 从 iris_rowkeys[id_] 中获取当前查询行的 iris_rowkey，并将其存储在 iris_rowkey 变量中
	Eigen::VectorXf iris_rowkey = iris_rowkeys[id_][cur_ptr]; // current query rowkey
    // 从 iris_features[id_] 中获取当前特征，并将其存储在 iris_feature 变量中。
	lidar_iris_descriptor::featureDesc iris_feature = iris_features[id_][cur_ptr]; // current feature

    // 检查 cur_ptr 是否小于排除最近帧数和候选帧数之和加 1
    // （exclude_recent_frame_num_ + candidates_num_ + 1）。
    // 如果是，直接返回默认的 result
	if(cur_ptr < exclude_recent_frame_num_ + candidates_num_ + 1)
	{
		return result; // early return 
	}

	// step 1: candidates from rowkey tree
	// kd tree construction
	Eigen::MatrixXf new_iris_rowkeys;
    // 创建一个空的 Eigen::MatrixXf 对象 new_iris_rowkeys，用于存储筛选后的 iris 行键。
	// int history_indexes = cur_ptr - exclude_recent_frame_num_;
	int cur_row = 0;     //初始化一个整数变量 cur_row，表示当前行的索引，初始值为 0。
	for (auto iris_rowkey : iris_rowkeys[id_])
	{
        // 检查 iris_rowkey 的第一个元素（帧索引）是否大于 cur_ptr - exclude_recent_frame_num_
        // 如果是，则跳过当前循环。
		if (iris_rowkey.first > cur_ptr - exclude_recent_frame_num_)
		{
			continue;
		}
        // 否则，通过调整 new_iris_rowkeys 的大小，
        // 将 iris_rowkey 的前 rows_ 行数据存储到 new_iris_rowkeys 中，
        // 并将其放置在 new_iris_rowkeys 的第 cur_row 列。
        // 增加 cur_row 的值。
		new_iris_rowkeys.conservativeResize(rows_, cur_row+1);
		new_iris_rowkeys.block(0, cur_row, rows_, 1) = iris_rowkey.second.block(0, 0, rows_, 1);
		cur_row++;
	}
	// new_iris_rowkeys.conservativeResize(rows_, history_indexes);
	// new_iris_rowkeys.block(0, 0, rows_, history_indexes) = iris_rowkeys[id_].block(0, 0, rows_, history_indexes);
	kdTree = Nabo::NNSearchF::createKDTreeTreeHeap(new_iris_rowkeys, rows_);

	// search n nearest neighbors
	Eigen::VectorXi indice(candidates_num_); //用于存储最近邻的索引。
	Eigen::VectorXf distance(candidates_num_);  //用于存储最近邻的距离。
	float min_distance = 10000000.0;   //表示初始的最小距离
	int min_index = -1;  //最小距离对应的索引，初始值为 -1
	int min_bias = 0;    //最小距离对应的偏移，初始值为 0

	// knn search
	kdTree->knn(iris_rowkey, indice, distance, candidates_num_);
    //调用 kdTree 的 knn 方法，进行最近邻搜索。
    // 传入参数包括 iris_rowkey（当前查询行键）、indice（用于存储最近邻的索引）、
    // distance（用于存储最近邻的距离）以及 candidates_num_（最近邻的数量）。

	// step 2: pairwise distance
    // 对每个最近邻的索引进行迭代，并计算当前特征与候选帧的相似度。
    // 通过比较相似度，找到最小距离对应的索引和偏移。
    // 如果找到的最小距离小于设定的距离阈值，更新 min_distance、min_index 和 min_bias 的值。
    // 这些结果将在后续的代码中用于阈值检查和结果返回
	for(int i = 0; i < std::min(candidates_num_, int(indice.size())); i++)
	{
		if(indice[i] >= indexes_maps[id_].size())
		{
			continue;
		}

		int bias;
		lidar_iris_descriptor::featureDesc candidate_iris_feature = iris_features[id_][indice[i]];
		float candidate_distance = compare(iris_feature, candidate_iris_feature, &bias);

		if(candidate_distance < min_distance)
		{
			min_distance = candidate_distance;
			min_index = indice[i];
			min_bias = bias;
		}
	}

	// threshold check
    // 根据最小距离和设定的距离阈值，确定内部循环闭合的状态，并返回相应的结果。
    // 检查最小距离 min_distance 是否小于设定的距离阈值 distance_threshold_。如果是，执行以下操作：
    // 将最小距离 min_distance 赋值给结果的第一个元素 result.first。
    // 将偏移 min_bias 赋值给结果的第二个元素 result.second。
    // 打印调试信息，显示内部循环闭合的状态、当前帧索引、最小距离和最小距离对应的候选帧索引。

	if(min_distance < distance_threshold_)
	{
		result.first = min_index;
		result.second = min_bias;
		ROS_DEBUG("\033[1;33m[Iris Intra Loop<%d>] btn %d and %d. Dis: %.2f.\033[0m",
			id_, cur_ptr, min_index, min_distance);
	}
    // 如果最小距离 min_distance 大于等于设定的距离阈值 distance_threshold_，执行以下操作：
    // 打印调试信息，显示未找到内部循环闭合的状态、当前帧索引、最小距离和最小距离对应的候选帧索引。
	else
	{
		ROS_DEBUG("\033[1;33m[Iris Intra Not loop<%d>] btn %d and %d. Dis: %.2f.\033[0m",
			id_, cur_ptr, min_index, min_distance);
	}
	return result;
}

// 用于检测激光雷达和视觉传感器之间的跨模组循环闭合
std::pair<int, float> lidar_iris_descriptor::detectInterLoopClosureID(
	const int cur_ptr)
{
	std::pair<int, float> result {-1, 0.0};
    // 获取当前帧的机器人ID和帧ID
	int robot_id = iris_feature_index_pairs[cur_ptr].first;
	int frame_id = iris_feature_index_pairs[cur_ptr].second;
    // 获取当前查询行键 iris_rowkey 和当前特征 iris_feature
	Eigen::VectorXf iris_rowkey = iris_rowkeys[robot_id][frame_id]; // current query rowkey
	lidar_iris_descriptor::featureDesc iris_feature = iris_features[robot_id][frame_id]; // current feature

	// step 1: candidates from rowkey tree
	// kd tree construction
	float min_distance = 10000000.0;
	int min_index = -1;
	int min_bias = 0;
	for(int i = 0; i < robot_num_; i++)
	{
        // 只有当 robot_id 和 id_ 同时相等或同时不相等时，循环中的剩余代码才会被执行
		if(robot_id == id_ && i == id_ || robot_id != id_ && i != id_)
		{
			continue;
		}

		Eigen::MatrixXf new_iris_rowkeys;//用于存储新的行键数据，其中每一行代表一个行键，每一列代表一个特征
		std::vector<int> new_indexes_maps;  //用于存储新的索引映射数据。它是一个整数向量，其中的每个元素对应一个索引映射
		std::vector<lidar_iris_descriptor::featureDesc> new_iris_features;
        // 用于存储激光雷达和视觉传感器的特征描述

		if(indexes_maps[i].size() > 0)
		{
            // 通过迭代iris_rowkeys[i]中的元素，将每个元素的值添加到new_iris_rowkeys矩阵的新列中
            // 为了调整矩阵的大小，使用conservativeResize函数将矩阵的列数增加1，
            // 然后使用block函数将新的iris_rowkey值复制到相应的位置上。
			for (auto iris_rowkey : iris_rowkeys[i])
			{
				int cur_row = new_iris_rowkeys.cols();
				new_iris_rowkeys.conservativeResize(rows_, cur_row + 1);
				new_iris_rowkeys.block(0, cur_row, rows_, 1) = iris_rowkey.second.block(0, 0, rows_, 1);
			}

            // 通过迭代indexes_maps[i]中的元素，将每个元素的值添加到new_indexes_maps向量中，
            // 使用emplace_back函数将元素添加到new_indexes_maps的末尾
			for (auto indexes_map : indexes_maps[i])
			{
				new_indexes_maps.emplace_back(indexes_map.second);
			}

            // 通过迭代iris_features[i]中的元素，将每个元素的值添加到new_iris_features向量中，
            // 使用emplace_back函数将元素添加到new_iris_features的末尾
			for (auto iris_feature : iris_features[i])
			{
				new_iris_features.emplace_back(iris_feature.second);
			}
		}

		if(new_indexes_maps.size() > candidates_num_ + 2)
		{
            // 使用Nabo::NNSearchF::createKDTreeLinearHeap函数创建一个基于KD树的最近邻搜索对象kdTree
            // 该树使用new_iris_rowkeys作为数据集，rows_作为数据集的维度参数
			kdTree = Nabo::NNSearchF::createKDTreeLinearHeap(new_iris_rowkeys, rows_);

			// search n nearest neighbors
            // 声明一个大小为candidates_num_的整数向量indice
            // 一个大小为candidates_num_的浮点数向量distance，用于存储最近邻搜索的结果。
			Eigen::VectorXi indice(candidates_num_);
			Eigen::VectorXf distance(candidates_num_);

			// knn search
            // 调用kdTree对象的knn方法进行K最近邻搜索，
            // 搜索结果将存储在indice和distance向量中，搜索的最近邻数量为candidates_num_
			kdTree->knn(iris_rowkey, indice, distance, candidates_num_);

			// step 2: pairwise distance
			// #pragma omp parallel for num_threads(4)
			for(int i = 0; i < std::min(candidates_num_, int(indice.size())); i++)
			{
                //检查当前索引是否超出了new_indexes_maps向量的大小。如果超出，则跳过当前循环，继续下一次迭代
				if(indice[i] >= new_indexes_maps.size())
				{
					continue;
				}

				int bias;//用于存储比较函数compare的输出
                // 从new_iris_features向量中获取最近邻帧的特征candidate_iris_feature
				lidar_iris_descriptor::featureDesc candidate_iris_feature = new_iris_features[indice[i]];
                // 调用compare函数，计算当前帧iris_feature与最近邻帧candidate_iris_feature之间的相似度，
                // 并将结果存储在candidate_distance中
				float candidate_distance = compare(iris_feature, candidate_iris_feature, &bias);

                // 如果candidate_distance小于min_distance，则更新min_distance、min_index和min_bias的值为当前最近邻帧的相应值。
				if(candidate_distance < min_distance)
				{
					min_distance = candidate_distance;
					min_index = new_indexes_maps[indice[i]];
					min_bias = bias;
				}
			}
		}
	}

	// threshold check
    // 如果 min_distance 小于设定的距离阈值 distance_threshold_，执行以下操作：
    // 将最小索引 min_index 和偏移 min_bias 分别赋值给结果的第一个和第二个元素。
    // 打印调试信息，显示跨模组循环闭合的状态、当前帧和最小距离对应的候选帧。
	if(min_distance < distance_threshold_)
	{
		result.first = min_index;
		result.second = min_bias;
		ROS_DEBUG("\033[1;33m[Iris Inter Loop<%d>] btn %d-%d and %d-%d. Dis: %.2f. Bias:%d\033[0m", id_,
			iris_feature_index_pairs[cur_ptr].first, iris_feature_index_pairs[cur_ptr].second,
			iris_feature_index_pairs[min_index].first, iris_feature_index_pairs[min_index].second, min_distance, min_bias);
	}
    // 否则，执行以下操作：
    // 打印调试信息，显示未找到跨模组循环闭合的状态、当前帧和最小距离对应的候选帧。
	else
	{
		ROS_DEBUG("\033[1;33m[Iris Inter Not loop<%d>] btn %d-%d and %d-%d. Dis: %.2f. Bias:%d\033[0m", id_,
			iris_feature_index_pairs[cur_ptr].first, iris_feature_index_pairs[cur_ptr].second,
			iris_feature_index_pairs[min_index].first, iris_feature_index_pairs[min_index].second, min_distance, min_bias);
	}
	return result;
}

// 是函数 getIndex 的实现，它用于根据给定的键（key）获取 iris 描述符的机器人标识符和描述符索引
std::pair<int8_t, int> lidar_iris_descriptor::getIndex(
	const int key)
{
    // 使用给定的 key 作为索引访问 iris_feature_index_pairs 容器，获取对应键值对
	return iris_feature_index_pairs[key];
}

// 是函数 getSize 的实现，用于获取 iris 描述符的大小（数量）。
int lidar_iris_descriptor::getSize(
	const int id = -1)
{
	if(id == -1)
	{
        //返回 iris_feature_index_pairs 容器的大小，即描述符的数量
		return iris_feature_index_pairs.size();
	}
	else
	{
        // 返回索引为 id 的机器人的描述符数量，即 indexes_maps[id] 容器的大小
		return indexes_maps[id].size();
	}
}
